#include <string>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>
#include <iostream>
#include<stdio.h>
#include<stdlib.h>
#include<string.h>
#include<errno.h>
#include<sys/types.h>
#include<sys/socket.h>
#include<unistd.h>
#include<netinet/in.h>
#include<string>
#include<map>
using namespace std;
#define MAXLINE 50

void encoding(char *p,int* motion){
    short* it=(short*)p;
    for(int i=0;i<20;i++){
        *it=motion[i];
        it++;
    }
}
void decoding(char *p,int* motion){
    short* it=(short*)p;
    for(int i=0;i<20;i++){
        motion[i]=*it;
        it++;
    }
}
void encilentcoding(string cmd,int t,int* motion,char *data)
{
    for(int i=0;i<3;i++)
        data[i]=cmd[i];
    data[3]='\0';
    for(int i=0;i<4;i++)
        data[i+4]=((char *)&t)[i];
    data[8]='\0';
    encoding(data+9,motion);

}

std::string joints[]={
"round_left_hip","left_hip_to_kneel","axis_to_left_hip","left_hip_to_shank","left_shank_to_foot","left_shank_pitch_foot",
"round_right_hip","right_hip_to_kneel","axis_to_right_hip","right_hip_to_shank","right_shank_to_foot","right_shank_pitch_foot"
};
string mapping[20]={
"0","left_hip_to_kneel","axis_to_left_hip","round_left_hip","left_hip_to_shank",
"left_shank_to_foot","left_shank_pitch_foot","0","right_hip_to_kneel","axis_to_right_hip",
"round_right_hip","right_hip_to_shank","right_shank_to_foot","right_shank_pitch_foot","0",
"0","0","0","0","0"
};
class joint_map{
public:
    static    double k; //to radius
    string joint_name;
    int motor;
    double angle;
    double get_angle(){
        angle= (motor-512)*k;
    return angle;
    }
};
double joint_map::k=300/1024*3.14/180;//scale to radius
	 int    listenfd, connfd;
    struct sockaddr_in     servaddr;
    char    buff[4096];
    int     np;
    void init_server(){
     if( (listenfd = socket(AF_INET, SOCK_STREAM, 0)) == -1 ){
    printf("create socket error: %s(errno: %d)\n",strerror(errno),errno);
    exit(0);
    }

    memset(&servaddr, 0, sizeof(servaddr));
    servaddr.sin_family = AF_INET;
    servaddr.sin_addr.s_addr = htonl(INADDR_ANY);
    servaddr.sin_port = htons(6666);

    if( bind(listenfd, (struct sockaddr*)&servaddr, sizeof(servaddr)) == -1){
    printf("bind socket error: %s(errno: %d)\n",strerror(errno),errno);
    exit(0);
    }

    if( listen(listenfd, 10) == -1){
    printf("listen socket error: %s(errno: %d)\n",strerror(errno),errno);
    exit(0);
    }

    }
int main(int argc, char** argv) {

    init_server();

    ros::init(argc, argv, "state_publisher");
    ros::NodeHandle n;
    ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1);
    tf::TransformBroadcaster broadcaster;
    ros::Rate loop_rate(300);

    const double degree = M_PI/180;

    // robot state

    // message declarations
    geometry_msgs::TransformStamped odom_trans;
    sensor_msgs::JointState joint_state;
    odom_trans.header.frame_id = "axis";
    odom_trans.child_frame_id = "base_link";

	int store_data[20];
		if( (connfd = accept(listenfd, (struct sockaddr*)NULL, NULL)) == -1){
            ROS_INFO("accept socket error: %s(errno: %d)",strerror(errno),errno);
            exit(1);
        }
    while (ros::ok()   )  {
            if(!recv(connfd, buff, MAXLINE, 0))
            {

                connfd = accept(listenfd, (struct sockaddr*)NULL, NULL) ;
            continue;
            }
                    string command=buff;
                  //  ROS_INFO("%s",buff+9);
                    int* time=(int*)(buff)+1;
                    decoding(buff+9,store_data);
                   // cout<<"cmd:"<<command<<endl;
                    //cout<<"time:"<<*time<<endl;

    for(int i=1;i<=6;i++)
        printf("%2d :%3d,",i,store_data[i]);
    cout<<endl;
    for(int i=8;i<=13;i++)
        printf("%2d :%3d,",i,store_data[i]);
    cout<<endl;
    ROS_INFO("line");
        //update joint_state

        joint_state.header.stamp = ros::Time::now();
        int n=sizeof(joints)/sizeof(joints[0]);
        joint_state.name.resize(n);
        joint_state.position.resize(n);
        double k=300.0/1024.0*3.1415926/180;

        int t=store_data[4];
        store_data[4]=-store_data[1]+store_data[4];
        store_data[5]+=t;


        t=store_data[11];
        store_data[11]=-store_data[8]+store_data[11];
        store_data[12]+=t;
        // ROS_INFO("11:%d",t);
        for(int i=0,j=0;i<20;i++){
                if(mapping[i]=="0")
                    continue;

            joint_state.name[j]=mapping[i];
            joint_state.position[j]=(double)(store_data[i])*300/1024*3.1415926/180;
            j++;
         //   ROS_INFO("%s:%d->%.3f,",mapping[i].data(), store_data[i],joint_state.position[j]);
        }






        // update transform
        // (moving in a circle with radius=2)

        odom_trans.header.stamp = ros::Time::now();
/*
        odom_trans.transform.translation.x = 1;
        odom_trans.transform.translation.y = 1;
        odom_trans.transform.translation.z = 1;*/
        odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(0);

        //send the joint state and transform
        joint_pub.publish(joint_state);
        broadcaster.sendTransform(odom_trans);

        // This will adjust as needed per iteration
        loop_rate.sleep();
    }


    return 0;

}
